﻿#pragma once
#include <QObject>
#include "../algorithmvisual.h"
#include "visual_data.h"
#include <qmath.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/transforms.h>
#include <pcl/common/centroid.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/types_c.h>

#include "AlgorithmPclPackage.h"
#include "AlgorithmVision3D.h"
#include "AlgorithmVision.h"


struct BOXINFO
{
	int SkuLength = 0;
	int SkuWidth = 0;
	int id = 0;
	SKUPOSITION Position;
	PointCloud::Ptr cloud;
	QVector<int> Child;
};
using namespace unstack;
class  AlgorithmUnstackCommon : public AlgorithmVisual
{
public:
	AlgorithmUnstackCommon();

	~AlgorithmUnstackCommon();

	virtual int Init(const visAlgParam& param) override;

	virtual bool Caculate(const VisAlgInput& input, Position& pos, VisAlgOutput& out) override;

private:
	PointCloud::Ptr pix2world(PointCloud::Ptr cloud);

	PointCloud::Ptr GetJigPoint();
	PointCloud::Ptr GetJigPoint(const JigSetting& info);

	int PointCloudInterferenceCheck(BOXINFO recog, PointCloud::Ptr sucker_convex, PointCloud::Ptr cloud_filter_ptr, float& delx, float& dely, cv::Mat& image);

	int getPointIntersection(PointCloud::Ptr point_hull, PointCloud::Ptr point_source);

	BOXINFO GetCaseInfoFromPix(PointCloud::Ptr cloud_case, RECGRESULT recg);

	BOXINFO GetCaseInfoFromPix2(PointCloud::Ptr cloud_case, RECGRESULT recg);

	PointCloud::Ptr Filter(PointCloud::Ptr cloud_in);

	QVector<PointCloud::Ptr> GetCaseCloudVec(PointCloud::Ptr cloud);

	PointCloud::Ptr GetCloudWithoutCase(BOXINFO caseinfo, PointCloud::Ptr cloud_ptr_world);

	PointCloud::Ptr GetCloudWithoutCaseVec(QVector<BOXINFO> caseinfo_vec, PointCloud::Ptr cloud_ptr_world);

	PointCloud::Ptr GetCloudWithCase(BOXINFO caseinfo, PointCloud::Ptr cloud_ptr_world);

	PointCloud::Ptr GetCloudWithCaseVec(QVector<BOXINFO> caseinfo_vec, PointCloud::Ptr cloud_ptr_world);

	int GetTopFloorCase(QVector<BOXINFO>& case_vec);

	int PointCloudCal(BOXINFO caseinfo, PointCloud::Ptr point_check_ptr, SKUPOSITION& res, cv::Mat& image);

	QVector<QVector<BOXINFO>> FindNeatCase(QVector<BOXINFO> const case_vec);

	int trySeveralGrab(QVector<QVector<BOXINFO>>& neatcase, PointCloud::Ptr cloud_world_ptr,
		const int max, const JigSetting& jig, GrabInfo& grab, BOXINFO& target);

	bool SetIntrinsic(QString ins);

	bool SetExternal(QString ext);

	bool SetWorkSpace(QString space);

	//更新上一次的数据  把本次需要拆垛的箱体的任意一个放入到结构体中  
	void UpdateBeforeinfo(BOXINFO reinfo, int dx, int dy, RECORDINFO& info);

	//计算高度差
	//file  深度图地址
	//info  上一次拆垛更新之后的结构体数据
	//fhigh 计算之后的高度差
	void CalPointDifference(QString file, RECORDINFO info, QString& ipt);

	//根据深度图转化的点云和坐标   切割点云
	PointCloud::Ptr GetCaseCloud(PointCloud::Ptr cloud, RECGRESULT result);

	void EuclideanClusterExtractionCloud(const PointCloud::Ptr& pCloud, float fClusterTolerance, int iMinClusterSize, int iMaxClusterSize, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>* pvCloudPtr);

	double getMaxCloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloudPtr);

	PointCloud::Ptr cam2pix(PointCloud::Ptr cloud);

	PointCloud::Ptr pix2cam(PointCloud::Ptr cloud);

	cv::RotatedRect getminAreaRect(cv::Mat Mask);

	void UpsampleMask(cv::Mat mask, cv::Mat& upMask, int pad);

	int BoxCheck(BOXINFO recog, PointCloud::Ptr cloud_filter_ptr);

	bool GetBoxSize(QString arr);

	void BlockCheck(QVector<QVector<BOXINFO>>& neatcase, int casenum);

	bool getBlockInfo(QVector<BOXINFO> block, BOXINFO& cases_one);

	float GetAngleTrans(float angle, float angleX, float angleY);

	bool SetUnstackSetting(const visAlgParam& param);

	void parrProcess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr pass_cloud);

	void DrawPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Scalar sc, cv::Mat& image);

	void DrawTray(cv::Mat& image);

	void DrowBox(cv::Mat& image, cv::RotatedRect rect, const int id);

	void DrowCoordinate(SKUPOSITION position, cv::Mat& rgb, float delx, float delty, int count);
	void DrawSucker(const Position& grab, const JigSetting& jig, cv::Mat& image);
	void DrowSucker(float angle, BOXINFO recog, float deltx, float delty, cv::Mat& image);
	int ParseRecogRecogResult(const QList<BoxRect>& input, QVector<RECGRESULT>& result);
	// 获取抓取位姿
	QVector<GrabInfo> GetGrabPosVec(BOXINFO& recog, const JigSetting& jig);
	GrabInfo GetGrabPosition(BOXINFO& recog, const JigSetting& jig, PointCloud::Ptr world);

	bool SortSkuPos(QVector<BoxInfo>& pos);
	bool SortGrabPos(QVector<GrabInfo>& pos);

private:

	RECORDINFO beforinfo;

	QVector<RECGRESULT> recog_result; // 识别结果，箱子框坐标

	QVector<QVector<float>> intrinsic_matrix;

	QVector<QVector<float>> external_matrix;

	bool bSave;

	pcl::visualization::PCLVisualizer::Ptr viewer;

	cv::Mat intrinsicMatrix;//内参

	cv::Mat distortionMatrix;//畸变系数

	cv::Mat externalMatrix;

	int boxLength, boxWidth, boxHeight = 0; //箱子长宽高，初始化为0

	double maxHeight = 0;

private://码垛配置
	//码垛配置功能参数
	//1.输送相对于托盘的位置
	//拆垛时先拆离输送位置近的箱子，再拆离输送位置远的箱子
	//具体来说需要判断拆垛方向
	int relativeX;//取值为0，1。0为从小到大拆，1 为从大到小拆
	int relativeY;//取值为0，1。0为从小到大拆，1 为从大到小拆

	//2.吸盘类型
	//纯吸盘和带侧边的吸盘
	int suckerType;//1为纯吸盘，2为带侧边吸盘

	//3.吸盘安装方式
	//纯吸盘时，将吸盘的长对应机械手X轴坐标，宽对应机械手Y坐标，此时示教器上显示的C角
	//带有侧边吸盘时， 将吸盘的长对应机械手X轴坐标，宽对应机械手Y坐标,同时按照拆垛方向，侧边朝向不被干涉的方向，此时示教器上显示的C角
	float angleX;
	float angleY;

	//4.吸盘尺寸
	QPointF suker_size;
	JigSetting m_jig = {};

	//5.最多抓取个数
	int GrabNum;

	//6.托盘坐标范围
	double xlimit_min;

	double xlimit_max;

	double ylimit_min;

	double ylimit_max;

	double zlimit_min;

	double zlimit_max;

	//7.是否启用箱子尺寸校验
	bool SizeCheck;

	//8.物料的最大最小尺寸
	int maxBoxLength;
	int maxBoxWidth;
	int minBoxLength;
	int minBoxWidth;

	int m_dir = -1;	// 抓取方向
	struct
	{
		bool offset;
		float x_min;
		float y_min;
		float x_max;
		float y_max;
		float x_off;
		float y_off;
	} m_offsetInfo = {};

	AlgorithmPclPackage pclClass;
	AlgorithmVision3D vision3d;
	AlgorithmVision vision;
};

template <typename T>
std::vector<int> sort_indexes(const std::vector<T>& v, int iflag = 0)
{
	// 初始化索引向量
	std::vector<int> idx(v.size());
	//使用iota对向量赋0~？的连续值
	std::iota(idx.begin(), idx.end(), 0);
	// 通过比较v的值对索引idx进行排序
	if (iflag == 0) {
		std::sort(idx.begin(), idx.end(), [&v](size_t i1, size_t i2) { return v[i1] < v[i2]; });
	} else {
		std::sort(idx.begin(), idx.end(), [&v](size_t i1, size_t i2) { return v[i1] > v[i2]; });
	}

	return idx;
}
